﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;
using System.Diagnostics;

namespace ImageProcessing
{
    public class EventArgsPTZ : EventArgs
    {
        public int Commond { get; private set; }
        public EventArgsPTZ(int cmd)
        {
            this.Commond = cmd;
        }
    }
    public static class FeatureExtraction
    {
        public static event EventHandler<EventArgsPTZ> OnPTZControl;

        unsafe delegate void ImageProcessCallback(byte* pIn, byte* pOut, Size imageIn, Size imageOut);

        private static void CheckInOut(object input, object output, string function)
        {
            if (input == null)
                throw new ApplicationException("input image is null|" + function);
            if (output == null)
                throw new ApplicationException("output image is null|" + function);
        }

        private static long BitmapToIntPtr(Bitmap imageIn, Bitmap imageOut, ImageProcessCallback imageProcess)
        {
            Stopwatch watch = Stopwatch.StartNew();

            //try
            //{
            unsafe
            {
                lock (imageOut)
                {
                    Rectangle rectIn = new Rectangle(0, 0, imageIn.Width, imageIn.Height);
                    Rectangle rectOut = new Rectangle(0, 0, imageOut.Width, imageOut.Height);

                    BitmapData dataIn = imageIn.LockBits(rectIn, ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
                    BitmapData dataOut = imageOut.LockBits(rectOut, ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);

                    byte* pIn = (byte*)(dataIn.Scan0.ToPointer());
                    byte* pOut = (byte*)(dataOut.Scan0.ToPointer());

                    if (imageProcess != null) imageProcess(pIn, pOut, imageIn.Size, imageOut.Size);

                    imageOut.UnlockBits(dataOut);
                    imageIn.UnlockBits(dataIn);
                }
            }
            //}
            //catch (Exception Ex)
            //{
            //    throw Ex;
            //}

            return watch.ElapsedMilliseconds;
        }

        public static long StableStream(Bitmap imageIn, Bitmap imageOut)
        {
            CheckInOut(imageIn, imageOut, "StableStream");

            if (StableBackground == null)
                StableBackground = new byte[imageOut.Width, imageOut.Height];

            unsafe
            {
                return BitmapToIntPtr(imageIn, imageOut, StableStreamCall);
            }
        }

        public static byte[,] StableBackground;
        unsafe private static void StableStreamCall(byte* pIn, byte* pOut, Size imageIn, Size imageOut)
        {
            for (int yOut = 0; yOut < imageOut.Height; yOut++)
            {
                for (int xOut = 0; xOut < imageOut.Width; xOut++)
                {
                    int index = (yOut * imageOut.Width + xOut) * 3;
                    byte key = pIn[index];

                    byte maxKey = StableBackground[xOut, yOut];

                    if (maxKey == 0) StableBackground[xOut, yOut] = key;
                    else if (key != maxKey) StableBackground[xOut, yOut] += (byte)((key - maxKey + 14) / 15);

                    pOut[index] =
                    pOut[index + 1] =
                    pOut[index + 2] =
                    Math.Min(Math.Max(StableBackground[xOut, yOut], byte.MinValue), byte.MaxValue);
                }
            }
        }

        public static byte[,] EliminateNoise;
        /// <summary>
        /// 提取与背景做差值，提取动态像素
        /// </summary>
        /// <param name="imageIn"></param>
        /// <param name="imageOut"></param>
        public static long Subtract(Bitmap imageIn, Bitmap imageOut)
        {
            CheckInOut(imageIn, imageOut, "StableStream");

            if (EliminateNoise == null)
                EliminateNoise = new byte[imageOut.Width, imageOut.Height];

            unsafe
            {
                return BitmapToIntPtr(imageIn, imageOut, SubtractCall);
            }
        }

        static bool startTrack = false;
        static bool initialTrack = false;
        static Rectangle rectTrack = new Rectangle();
        static Point[] pointFeatures = new Point[0];        //特征点
        static byte[] mouldFeature = new byte[0];
        static Color colorTrack = Color.Yellow;
        static readonly Size SizeAvailable = new Size(10, 10);      //运动框有效大小
        static readonly Size SizePadding = new Size(50, 50);        //内有效距离
        static readonly Rectangle RectParticle = new Rectangle(-25, -25, 50, 50);//粒子范围
        static int intersectNumber = 25;
        static int commod = 0;
        static int commodCount = 0;
        unsafe private static void SubtractCall(byte* pIn, byte* pOut, Size imageIn, Size imageOut)
        {
            Point[] particles = new Point[0];
            #region 开始跟踪
            if (startTrack)
            {
                if (!initialTrack)
                {//初始化
                    //提取特征点
                    pointFeatures = GetRandomPoint(
                        new Rectangle(new Point(), rectTrack.Size),
                        500);
                    //提取模板特征直方图
                    mouldFeature = GetFeatureHistogram(rectTrack, pIn, imageOut, pointFeatures);

                    initialTrack = true;
                }
                else
                {
                    //重采样，重新获取粒子坐标
                    Rectangle rectParticle = new Rectangle(
                        rectTrack.Left + RectParticle.X,
                        rectTrack.Top + RectParticle.Y,
                        RectParticle.Width, RectParticle.Height);
                    particles = GetRandomPoint(rectParticle, 20);

                    //int[] features = new int[particles.Length];
                    Point pointMaxFeature = rectTrack.Location;
                    int maxFeature = 0;
                    byte[] particleMaxFeature = new byte[0];


                    //求最大特征
                    for (int i = 0; i < particles.Length; i++)
                    {
                        //获取粒子特征
                        byte[] particleFeature = GetFeatureHistogram(
                            new Rectangle(particles[i], rectTrack.Size), pIn, imageOut, pointFeatures);
                        //获取粒子特征与模板特征的相似度
                        int feature = GetSimilarFeature(mouldFeature, particleFeature);
                        if (feature > maxFeature)
                        {
                            maxFeature = feature;
                            pointMaxFeature = particles[i];
                            particleMaxFeature = particleFeature;
                        }
                    }

                    int[] similars = new int[particles.Length];
                    //求平均特征
                    int sumSimilars = 0;
                    int avgFeature = 0;
                    for (int i = 0; i < particles.Length; i++)
                    {
                        //获取粒子特征直方图
                        byte[] particleFeature = GetFeatureHistogram(
                            new Rectangle(particles[i], rectTrack.Size), pIn, imageOut, pointFeatures);
                        //获取粒子特征与模板特征的相似度
                        similars[i] = GetSimilarFeature(mouldFeature, particleFeature);
                        sumSimilars += similars[i];
                    }
                    Point pointAvgFeature = new Point();
                    for (int i = 0; i < particles.Length; i++)
                    {
                        pointAvgFeature.X += particles[i].X * similars[i] / sumSimilars;
                        pointAvgFeature.Y += particles[i].Y * similars[i] / sumSimilars;
                    }
                    particleMaxFeature = GetFeatureHistogram(
                            new Rectangle(pointAvgFeature, rectTrack.Size), pIn, imageOut, pointFeatures);
                    avgFeature = GetSimilarFeature(mouldFeature, particleMaxFeature);
                    rectTrack.Location = maxFeature > avgFeature ? pointMaxFeature : pointAvgFeature;

                    ////修正特征模板
                    //for (int i = 0; i < mouldFeature.Length; i++)
                    //{
                    //    if (particleMaxFeature[i] > mouldFeature[i])
                    //        mouldFeature[i]++;
                    //    else if (particleMaxFeature[i] < mouldFeature[i])
                    //        mouldFeature[i]--;
                    //}

                    

                    Rectangle rectAvailable = new Rectangle(new Point(), imageOut);
                    rectAvailable.Inflate(-SizePadding.Width, -SizePadding.Width);
                    if (!rectAvailable.Contains(rectTrack)
                        || maxFeature < 200 * 256
                        || intersectNumber <= 0)
                    {
                        initialTrack = false;
                        startTrack = false;
                        rectTrack = new Rectangle();
                        pointFeatures = new Point[0];
                        mouldFeature = new byte[0];
                        colorTrack = Color.FromArgb(
                            random.Next(256),
                            random.Next(256),
                            random.Next(256));
                        intersectNumber = 50;
                        commodCount = 0;
                        if (OnPTZControl != null)
                        {
                            commod = 0;
                            OnPTZControl(null, new EventArgsPTZ(0));
                        }
                    }
                    //else
                    //{
                    //    //左
                    //    if (rectTrack.Left < imageOut.Width / 4)
                    //    {
                    //        if ((commod & 1) == 0) commod |= 1;
                    //    }
                    //    //右
                    //    else if (rectTrack.Right > imageOut.Width * 3 / 4)
                    //    {
                    //        if ((commod & 2) == 0) commod |= 2;
                    //    }

                    //    //上
                    //    if (rectTrack.Top < imageOut.Height / 4)
                    //    {
                    //        if ((commod & 4) == 0) commod |= 4;
                    //    }
                    //    //左
                    //    else if (rectTrack.Bottom < imageOut.Height * 3 / 4)
                    //    {
                    //        if ((commod & 8) == 0) commod |= 8;
                    //    }

                    //    if (commod > 0) commodCount++;

                    //    if (OnPTZControl != null &&  commodCount >= 100)
                    //    OnPTZControl(null, new EventArgsPTZ(commod));

                    //}
                }

                //DrawRectangle(rectTrack, Color.Yellow, pOut, imageOut);

            }
            #endregion


            #region 运动标记
            for (int yOut = 0; yOut < imageOut.Height; yOut++)
            {
                for (int xOut = 0; xOut < imageOut.Width; xOut++)
                {
                    int index = (yOut * imageOut.Width + xOut) * 3;
                    pOut[index] =
                    (byte)(Math.Abs(StableBackground[xOut, yOut]
                    - pIn[(yOut * imageOut.Width + xOut) * 3]) > 50 ? 255 : 0);
                }
            }
            #endregion

            List<Rectangle> rectActive = new List<Rectangle>();
            #region 矩形扩张
            for (int yOut = 0; yOut < imageOut.Height; yOut++)
            {
                for (int xOut = 0; xOut < imageOut.Width; xOut++)
                {
                    int index = GetIndex(xOut, yOut, imageOut.Width);
                    if (pOut[index] == 255)
                    {
                        Rectangle rect = new Rectangle(xOut, yOut, 1, 1);
                        //向右延伸
                        for (int i = rect.Right; i < imageOut.Width; i++)
                        {
                            if (pOut[GetIndex(i, yOut, imageOut.Width)] != 255)
                            {
                                rect.Width = i - xOut;
                                break;
                            }
                        }
                        //向下延伸
                        for (int i = rect.Bottom; i < imageOut.Height; i++)
                        {
                            int count = 0;
                            for (int j = rect.Left; j < rect.Right; j++)
                            {
                                if (pOut[GetIndex(j, i, imageOut.Width)] == 255)
                                {
                                    count++;
                                    break;
                                }
                            }
                            if (count == 0)
                            {
                                rect.Height = i - yOut;
                                break;
                            }
                        }
                        //向左延伸
                        for (int i = rect.Left - 1; i > 0; i--)
                        {
                            int count = 0;
                            for (int j = rect.Top; j < rect.Bottom; j++)
                            {
                                if (pOut[GetIndex(i, j, imageOut.Width)] == 255)
                                {
                                    count++;
                                    break;
                                }
                            }
                            if (count == 0)
                            {
                                rect.X = i;
                                rect.Width += rect.Left - i;
                                break;
                            }
                        }
                        //向右延伸
                        for (int i = rect.Right; i < imageOut.Width; i++)
                        {
                            int count = 0;
                            for (int j = rect.Top; j < rect.Bottom; j++)
                            {
                                if (pOut[GetIndex(i, j, imageOut.Width)] == 255)
                                {
                                    count++;
                                    break;
                                }
                            }
                            if (count == 0)
                            {
                                rect.Width = i - rect.Left;
                                break;
                            }
                        }
                        //向下延伸
                        for (int i = rect.Bottom; i < imageOut.Height; i++)
                        {
                            int count = 0;
                            for (int j = rect.Left; j < rect.Right; j++)
                            {
                                if (pOut[GetIndex(j, i, imageOut.Width)] == 255)
                                {
                                    count++;
                                    break;
                                }
                            }
                            if (count == 0)
                            {
                                rect.Height = i - yOut;
                                break;
                            }
                        }

                        if (rect.Width > SizeAvailable.Width
                            && rect.Height > SizeAvailable.Height)
                        {
                            rectActive.Add(rect);

                            for (int y = rect.Top; y < rect.Bottom; y++)
                            {
                                for (int x = rect.Left; x < rect.Right; x++)
                                {
                                    //标记运动的点
                                    int idx = GetIndex(x, y, imageOut.Width);
                                    if (pOut[idx] == 255)
                                    {
                                        pOut[idx] = 0;
                                        pOut[idx + 1] = 255;
                                    }

                                }
                            }
                        }


                    }
                }
            }
            #endregion

            bool intersectExsit = false;//没有一个相交
            #region 合并相交矩形
            do
            {
                intersectExsit = false;
                for (int i = 0; i < rectActive.Count - 1; i++)
                {
                    bool intersect = false;//是否与其它相交
                    for (int j = i + 1; j < rectActive.Count; j++)
                    {
                        Rectangle rect1 = rectActive[i];
                        Rectangle rect2 = rectActive[j];
                        rect1.Inflate(10, 10);
                        rect2.Inflate(10, 10);
                        if (rect1.IntersectsWith(rect2))
                        {
                            rect1.Inflate(-10, -10);
                            rect2.Inflate(-10, -10);
                            //int x = Math.Min(rect1.Left, rect2.Left);
                            //int y = Math.Min(rect1.Top, rect2.Top);
                            //int w = Math.Max(rect1.Right, rect2.Right) - x;
                            //int h = Math.Max(rect1.Bottom, rect2.Bottom) - y;
                            rectActive.Remove(rect1);
                            rectActive.Remove(rect2);
                            rectActive.Add(Rectangle.Union(rect1, rect2));
                            //rectActive.Add(new Rectangle(x, y, w, h));
                            intersect = true;
                            break;
                        }
                    }
                    if (intersect)
                    {
                        intersectExsit = true;
                        break;
                    }
                }

            } while (intersectExsit);
            #endregion

            Rectangle rectMax = new Rectangle();
            #region 获取最大矩形
            bool isIntersect = false;
            foreach (var item in rectActive)
            {
                if (item.Left < 50 || item.Top < 50
                    || imageOut.Width - item.Right < 50
                    || imageOut.Height - item.Bottom < 50)
                    continue;
                if (rectMax.Width == 0 && rectMax.Height == 0
                    || item.Width * item.Height > rectMax.Width * rectMax.Height)
                    rectMax = item;

                if (item.IntersectsWith(rectTrack))
                    isIntersect = true;
            }
            if (!isIntersect) intersectNumber--;
            if (!startTrack)
            {
                if (rectMax != new Rectangle())
                {
                    startTrack = true;
                    //rectMax.Inflate(25, 25);
                    rectTrack = rectMax;
                }
            }

            #endregion

            #region 绘图
            foreach (var rect in rectActive)
            {
                //for (int y = rect.Top; y < rect.Bottom; y++)
                //{
                //    for (int x = rect.Left; x < rect.Right; x++)
                //    {
                //        //标记运行区域的点
                //        int index = GetIndex(x, y, imageOut.Width);
                //        if (pOut[index] == 255)
                //        {
                //            pOut[index] =
                //            pOut[index + 1] = 
                //            pOut[index + 2] = pIn[index];
                            
                //        }
                //    }
                //}

                //绘制动态框
                DrawRectangle(rect, Color.DarkCyan, pOut, imageOut);
            }

            //绘制跟踪框
            if (rectTrack.Width > 0 && rectTrack.Height > 0)
                DrawRectangle(rectTrack, colorTrack, pOut, imageOut);

            if (startTrack)
            {
                //绘制粒子
                foreach (var p in particles)
                {
                    int index = GetIndex(p.X, p.Y, imageOut.Width);
                    pOut[index + 1] =
                    pOut[index + 2] = 255;
                }
                //绘制粒子框
                //for (int i = 0; i < particles.Length; i++)
                //{
                //    Rectangle rectParticle = new Rectangle(particles[i], rectTrack.Size);
                //    DrawRectangle(rectParticle, colorTrack, pOut, imageOut);
                //}
            }
            #endregion


        }

        unsafe static int GetSimilarFeature(byte[] feature1, byte[] feature2)
        {
            if (feature1 == null || feature2 == null)
                throw new Exception("feature is null");
            //if (feature1.Length == 0 || feature2.Length == 0)
            //throw new Exception("feature's length is zero");

            int sum = 0;
            for (int i = 0; i < Math.Min(feature1.Length, feature2.Length); i++)
            {
                sum += byte.MaxValue - Math.Abs(feature1[i] - feature2[i]);
            }
            return sum;// / Math.Min(feature1.Length, feature2.Length);
        }

        unsafe static byte[] GetFeatureHistogram(Rectangle rect, byte* pBuffer, Size sizeBuffer, Point[] pointFeature)
        {
            byte[] greyHistogram = new byte[byte.MaxValue + 1];

            foreach (var item in pointFeature)
            {
                byte grey = pBuffer[GetIndex(rect.X + item.X, rect.Y + item.Y, sizeBuffer.Width)];
                greyHistogram[grey]++;
            }

            return greyHistogram;
        }

        unsafe static void DrawRectangle(Rectangle rect, Color color, byte* pBuffer, Size sizeBuffer)
        {
            for (int i = rect.Left; i < rect.Right; i++)
            {
                int indexTop = GetIndex(i, rect.Top, sizeBuffer.Width);
                int indexBottom = GetIndex(i, rect.Bottom - 1, sizeBuffer.Width);
                pBuffer[indexTop + 0] = color.B;
                pBuffer[indexTop + 1] = color.G;
                pBuffer[indexTop + 2] = color.R;
                pBuffer[indexBottom + 0] = color.B;
                pBuffer[indexBottom + 1] = color.G;
                pBuffer[indexBottom + 2] = color.R;
            }

            for (int i = rect.Top; i < rect.Bottom; i++)
            {
                int indexLeft = GetIndex(rect.Left, i, sizeBuffer.Width);
                int indexRight = GetIndex(rect.Right - 1, i, sizeBuffer.Width);
                pBuffer[indexLeft + 0] = color.B;
                pBuffer[indexLeft + 1] = color.G;
                pBuffer[indexLeft + 2] = color.R;
                pBuffer[indexRight + 0] = color.B;
                pBuffer[indexRight + 1] = color.G;
                pBuffer[indexRight + 2] = color.R;
            }
        }

        static int GetIndex(int x, int y, int w) { return (y * w + x) * 3; }

        static Point[] GetRandomPoint(Rectangle rect, int count)
        {
            if (rect.Width == 0 || rect.Height == 0) throw new ApplicationException("width or height is zero");
            PointF[] pointfs = new PointF[count];
            Point[] points = new Point[count];
            RectangleF rectZero = new RectangleF(-(float)rect.Width / 2, -(float)rect.Height / 2, rect.Width, rect.Height);
            for (int i = 0; i < count; i++)
            {
                do
                {
                    pointfs[i] = GetRandomNormalDistrbution();
                    pointfs[i].X *= (float)rect.Width / 4;
                    pointfs[i].Y *= (float)rect.Height / 4;
                    pointfs[i].X += (float)rect.Width / 2;
                    pointfs[i].Y += (float)rect.Height / 2;
                } while (pointfs[i].X < 0 || pointfs[i].X >= rect.Width - 1
                        || pointfs[i].Y < 0 || pointfs[i].Y >= rect.Height - 1);

                points[i].X = Convert.ToInt32(rect.X + pointfs[i].X);
                points[i].Y = Convert.ToInt32(rect.Y + pointfs[i].Y);
            }



            return points;
        }

        /*  二维正态分布随机数
         *  Box–Muller转换公式
         *  A. 标准形式
         *      假设U1 和 U2 是 (0, 1] 均匀分布的随机数，且U1 和 U2 彼此独立，令：
         *      Z0 = Sqrt(-2 * In(U1)) * cos(2 * Pi * U2)
         *      Z1 = Sqrt(-2 * In(U1)) * sin(2 * Pi * U2)
         *      那么 Z0 和 Z1 就是服从N(0,1)的随机数，且 Z0 和 Z1 彼此独立
         *  B.极坐标形式
         *      假设u 和 v 是 [-1, 1] 均匀分布的随机量，且u 和 v 彼此独立，令：
         *      s = u*u + v*v
         *      那么随机数z0 和 z1可以按照如下公式产生
         *      z0 = Sqrt(-2 * In(s)) * u / Sqrt(s);
         *      z1 = Sqrt(-2 * In(s)) * v / Sqrt(s);
         *      结果是z0 和 z1服从N(0,1)的随机数，且 z0 和 z1彼此独立
         */
        static Random random = new Random();
        static PointF GetRandomNormalDistrbution()
        {
            double u, v, s = 0, x = 0, y = 0, temp;
            do
            {
                u = random.NextDouble();
                v = random.NextDouble();
                u = 2 * u - 1;
                v = 2 * v - 1;
                s = u * u + v * v;
            } while (s > 1 || s == 0);
            temp = Math.Sqrt(-2 * Math.Log(s) / s);
            x = temp * u;
            y = temp * v;

            return new PointF(Convert.ToSingle(x), Convert.ToSingle(y));
        }
    }
}
